#include "boids.h"

__device__ float4 *elem;
__device__    int *bitmap;

__device__ float getDistance(float4 a, float4 b)
{
	const float dX = a.x - b.x;
	const float dY = a.y - b.y;

	return sqrt(dX * dX + dY * dY);
}

__device__ GLfloat getArea(int N, int32_t *nList)
{
	int i;
	GLfloat areaTimes2 = 0.0;

	for (i = 0; i < N - 1; i++) {
		areaTimes2 += ((elem[nList[i]].x * elem[nList[i + 1]].y) 
			       - (elem[nList[i + 1]].x * elem[nList[i]].y));
	}

	areaTimes2 += ((elem[nList[N - 1]].x * elem[nList[0]].y) 
		       - (elem[nList[0]].x * elem[nList[N - 1]].y));

	return (areaTimes2 / 2);
}

__device__ float4 getCentroid(int N, int32_t *nList)
{
	GLfloat aggX = 0.0;
	GLfloat aggY = 0.0;
	float4 c;

	if (N == 1) {
		c.x = elem[nList[0]].x;
		c.y = elem[nList[0]].y;
	}

	GLfloat A = getArea(N, nList);

	if (A == 0.0)
		A = 1.0;

	c.x = 0.0;
	c.y = 0.0;

	for (int i = 0; i < N - 1; i++) {
		const GLfloat commonTerm  = (elem[nList[i]].x * elem[nList[i + 1]].y) 
			- (elem[nList[i + 1]].x * elem[nList[i]].y);
		aggX += (elem[nList[i]].x + elem[nList[i + 1]].x) * commonTerm;
		aggY += (elem[nList[i]].y + elem[nList[i + 1]].y) * commonTerm;
	}

	const GLfloat commonTerm  = (elem[nList[N - 1]].x * elem[nList[0]].y) 
		- (elem[nList[0]].x * elem[nList[N - 1]].y);
	aggX += (elem[nList[N - 1]].x + elem[nList[0]].x) * commonTerm;
	aggY += (elem[nList[N - 1]].y + elem[nList[0]].y) * commonTerm;

	c.x = aggX / (6 * A);
	c.y = aggY / (6 * A);

	return c;
}

__device__ void avoidCollision(float4 *me)
{
	float fi = (me->x + 1.0) * 1024;
	float fj = (me->y + 1.0) * 1024;

	int i = (int)fi;
	int j = (int)fj;

	if (!bitmap[i * 2048 + j])
		return;

	if(me->z == NORTH) {
		me->y -= BOID_SPEED;
		me->z = WEST;
	}
  
	else if (me->z == SOUTH) {
	  	me->y += BOID_SPEED;
	  	me->z = EAST;
	}
    
  	else if (me->z == EAST) {
    		me->x -= BOID_SPEED;
		me->z = NORTH;
  	}
  	else if (me->z == WEST) {
    		me->x += BOID_SPEED;
		me->z = SOUTH;
  	}
}

__device__ void moveToward(float4 *me, float4 *t)
{
	const float dX = me->x - t->x;
	const float dY = me->y - t->y;

	if(dX > SPACE) {
		me->z = WEST;
		me->x -= BOID_SPEED;
	} else if (dX < -SPACE) {
		me->z = EAST;
		me->x += BOID_SPEED;
	}

	if(dY > SPACE) {
		me->z = SOUTH;
		me->y -= BOID_SPEED;
	} else if (dY < -SPACE) {
		me->z = NORTH;
		me->y += BOID_SPEED;
	}
}

__device__ void moveForward(float4 *me)
{
  	if(me->z == NORTH) 
		me->y += BOID_SPEED;
  
	else if (me->z == SOUTH) 
		me->y -= BOID_SPEED;
    
	else if (me->z == EAST) 
		me->x += BOID_SPEED;
	
	else if (me->z == WEST) 
		me->x -= BOID_SPEED;
}

__device__ void computeHumanActivity(float4 *me)
{
	int32_t nListID[N_LIMIT];
	unsigned int nCount = 0;
	int flagDanger = 0;
	// int dangerID = -1;

	for (int i = 0; i < BOID_N_BODIES; i++) {
		if (nCount >= N_LIMIT)
			break;

		float d = getDistance(*me, elem[i]);

		if (d <= NDIST && d != 0) {
			nListID[nCount++] = i;

			if(elem[i].w == STATE_GHOUL) {
				flagDanger = 1;
				me->w = STATE_PANIC;
				// dangerID = i;

				if(d <= GHOUL_ATTACK_LIMIT) {
					me->w = STATE_GHOUL;
					return;
				}
			}
		}
	}

	if(nCount > 0) {
		float4 c = getCentroid(nCount, nListID);

		if(flagDanger) {
			c.x = c.x * -1;
			c.y = c.y * -1;
		} 

		moveToward(me, &c);
	}

	if(!flagDanger) me->w = STATE_HUMAN;

	moveForward(me);
}

__device__ void computeCopActivity(float4 *me)
{
	float minDist = 5.0;
	int nnID = -1;
	int candidateID = -1;

	for (int i = 0; i < BOID_N_BODIES; i++) {
		float d = getDistance(*me, elem[i]);

		if(d <= COP_CANDIDATE_LIMIT && elem[i].w == STATE_GHOUL) { 
			candidateID = i; 

			if(d < minDist) {
				minDist = d;
				nnID = i;
			}
		}

		if(elem[i].w == STATE_GHOUL && d <= COP_ATTACK_LIMIT) {
			elem[i].w = STATE_DEAD;
		}
	}

	if(candidateID != -1) {
		moveToward(me, &elem[nnID]);
	}

	moveForward(me);
}

__device__ void computeGhoulActivity(float4 *me)
{
	float minDist = 5.0;
	int nnID = -1;
	int candidateID = -1;

	for (int i = 0; i < BOID_N_BODIES; i++) {
		float d = getDistance(*me, elem[i]);

		if(d <= GHOUL_CANDIDATE_LIMIT && elem[i].w == STATE_HUMAN) { 
			candidateID = i; 

			if(d < minDist) {
				minDist = d;
				nnID = i;
			}
		}

		if(elem[i].w == STATE_COP && d <= COP_ATTACK_LIMIT) {
			me->w = STATE_DEAD;
		}
	}

	if(candidateID != -1) {
		moveToward(me, &elem[nnID]);
	}

	if(nnID != -1) {
		moveToward(me, &elem[nnID]);
	}

	moveForward(me);
}

__global__ void computeKernel_device(float4 *dptr, float4 *d_elem, 
				     int *d_bitmap, unsigned int nBodies) 
{
	int index = blockDim.x * blockIdx.x + threadIdx.x;

	elem = d_elem;
	bitmap = d_bitmap;

	float4 *me = &d_elem[index];
	const float deg135 = (float) me->z + 3 * PI / 4;
	const float ndeg135 = (float) me->z - 3 * PI / 4;
	const float l = 0.005f;
	const float b = (float) l / 4;

	// avoidCollision(me);

	if(me->w == STATE_HUMAN || me->w == STATE_PANIC)
		computeHumanActivity(me);
	else if(me->w == STATE_GHOUL)
		computeGhoulActivity(me);
	else if(me->w == STATE_COP)  
		computeCopActivity(me);

	float u = me->x;
	float v = me->y;

	float x1 = u + (l * cosf((float)me->z));
	float y1 = v + (l * sinf((float)me->z));

	float x2 = u + (b * (-cosf(deg135) - sinf(deg135)));
	float y2 = v + (b * (sinf(deg135) - cosf(deg135)));

	float x3 = u + (b * (-cosf(ndeg135) + sinf(ndeg135)));
	float y3 = v + (b * (sinf(ndeg135) + cosf(ndeg135)));

	index = index * 3;

	dptr[index]     = make_float4(x1, y1, 1.0f, 1.0f);
	dptr[index + 1] = make_float4(x2, y2, 1.0f, 1.0f);
	dptr[index + 2] = make_float4(x3, y3, 1.0f, 1.0f);

	const float4 human_color = make_float4(0.0f, 1.0f, 0.0f, 1.0f);
	const float4 panic_color = make_float4(1.0f, 0.5f, 0.0f, 1.0f);
	const float4 ghoul_color = make_float4(1.0f, 0.0f, 0.0f, 1.0f);
	const float4   cop_color = make_float4(0.0f, 0.0f, 1.0f, 1.0f);
	const float4  dead_color = make_float4(0.5f, 0.5f, 0.5f, 1.0f);

	if (me->w == STATE_HUMAN) {
		dptr[index + (nBodies * 3)] = 
			dptr[index + (nBodies * 3) + 1] = 
			dptr[index + (nBodies * 3) + 2] = human_color;
	} else if (me->w == STATE_GHOUL) {
		dptr[index + (nBodies * 3)] = 
			dptr[index + (nBodies * 3) + 1] = 
			dptr[index + (nBodies * 3) + 2] = ghoul_color;
	} else if (me->w == STATE_COP) {
		dptr[index + (nBodies * 3)] = 
			dptr[index + (nBodies * 3) + 1] = 
			dptr[index + (nBodies * 3) + 2] = cop_color;
	} else if (me->w == STATE_PANIC) {
		dptr[index + (nBodies * 3)] = 
			dptr[index + (nBodies * 3) + 1] = 
			dptr[index + (nBodies * 3) + 2] = panic_color;
	} else if (me->w == STATE_DEAD) {
		dptr[index + (nBodies * 3)] = 
			dptr[index + (nBodies * 3) + 1] = 
			dptr[index + (nBodies * 3) + 2] = dead_color;
	}
}

extern "C" void computeKernel(float4 *dptr, float4 *d_elem, 
			      int *d_bitmap, unsigned int nBodies)
{
  	int threadsPerBlock = 256;
	int blocksPerGrid = (nBodies + threadsPerBlock - 1) / threadsPerBlock;
	computeKernel_device<<<blocksPerGrid, threadsPerBlock>>>(dptr, d_elem, 
								 d_bitmap, nBodies);
}
